#!/usr/bin/env python3

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch.actions import DeclareLaunchArgument, TimerAction
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition

def generate_launch_description():
    # 声明启动参数
    test_mode_arg = DeclareLaunchArgument(
        'test_mode',
        default_value='high_frequency',
        description='Test mode: high_frequency, stress_test, or latency_test'
    )

    message_rate_arg = DeclareLaunchArgument(
        'message_rate',
        default_value='100.0',
        description='Message publishing rate in Hz'
    )

    # 高频通信测试
    high_frequency_container = ComposableNodeContainer(
        name='high_frequency_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            # 高频发布者
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentPublisher',
                name='high_freq_publisher',
                parameters=[{
                    'publish_frequency': LaunchConfiguration('message_rate'),
                    'message_prefix': 'HighFreq'
                }],
                remappings=[
                    ('component_topic', 'high_freq_data')
                ]
            ),

            # 快速处理器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentProcessor',
                name='fast_processor',
                parameters=[{
                    'processing_delay_ms': 1,  # 最小延迟
                    'add_timestamp': False
                }],
                remappings=[
                    ('raw_data', 'high_freq_data')
                ]
            ),

            # 快速聚合器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentAggregator',
                name='fast_aggregator',
                parameters=[{
                    'aggregation_window_ms': 100,  # 短窗口
                    'max_messages_per_batch': 50   # 大批次
                }]
            ),

            # 性能监控器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentMonitor',
                name='performance_monitor',
                parameters=[{
                    'monitoring_interval_ms': 1000,  # 高频监控
                    'alert_threshold': 1000
                }]
            )
        ],
        output='screen',
        condition=IfCondition(LaunchConfiguration('test_mode'))
    )

    # 压力测试 - 多个发布者
    stress_test_container = ComposableNodeContainer(
        name='stress_test_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            # 多个发布者
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentPublisher',
                name='stress_publisher_1',
                parameters=[{
                    'publish_frequency': 50.0,
                    'message_prefix': 'Stress1'
                }],
                remappings=[
                    ('component_topic', 'stress_data_1')
                ]
            ),
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentPublisher',
                name='stress_publisher_2',
                parameters=[{
                    'publish_frequency': 50.0,
                    'message_prefix': 'Stress2'
                }],
                remappings=[
                    ('component_topic', 'stress_data_2')
                ]
            ),
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentPublisher',
                name='stress_publisher_3',
                parameters=[{
                    'publish_frequency': 50.0,
                    'message_prefix': 'Stress3'
                }],
                remappings=[
                    ('component_topic', 'stress_data_3')
                ]
            ),

            # 多路处理器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentProcessor',
                name='stress_processor_1',
                parameters=[{
                    'processing_delay_ms': 10,
                    'add_timestamp': True
                }],
                remappings=[
                    ('raw_data', 'stress_data_1'),
                    ('processed_data', 'processed_stress_1')
                ]
            ),
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentProcessor',
                name='stress_processor_2',
                parameters=[{
                    'processing_delay_ms': 10,
                    'add_timestamp': True
                }],
                remappings=[
                    ('raw_data', 'stress_data_2'),
                    ('processed_data', 'processed_stress_2')
                ]
            ),
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentProcessor',
                name='stress_processor_3',
                parameters=[{
                    'processing_delay_ms': 10,
                    'add_timestamp': True
                }],
                remappings=[
                    ('raw_data', 'stress_data_3'),
                    ('processed_data', 'processed_stress_3')
                ]
            ),

            # 统一监控器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentMonitor',
                name='stress_monitor',
                parameters=[{
                    'monitoring_interval_ms': 500,
                    'alert_threshold': 500
                }]
            )
        ],
        output='screen',
        condition=IfCondition(LaunchConfiguration('test_mode'))
    )

    # 延迟测试 - 精确延迟测量
    latency_test_container = ComposableNodeContainer(
        name='latency_test_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            # 延迟测试发布者
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentPublisher',
                name='latency_publisher',
                parameters=[{
                    'publish_frequency': 10.0,
                    'message_prefix': 'LatencyTest'
                }],
                remappings=[
                    ('component_topic', 'latency_data')
                ]
            ),

            # 零延迟处理器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentProcessor',
                name='zero_latency_processor',
                parameters=[{
                    'processing_delay_ms': 0,  # 零延迟
                    'add_timestamp': True
                }],
                remappings=[
                    ('raw_data', 'latency_data')
                ]
            ),

            # 延迟监控器
            ComposableNode(
                package='multi_launch_demo',
                plugin='multi_launch_demo::ComponentMonitor',
                name='latency_monitor',
                parameters=[{
                    'monitoring_interval_ms': 1000,
                    'alert_threshold': 100
                }]
            )
        ],
        output='screen',
        condition=IfCondition(LaunchConfiguration('test_mode'))
    )

    return LaunchDescription([
        test_mode_arg,
        message_rate_arg,
        high_frequency_container,
        stress_test_container,
        latency_test_container
    ])
